#include <math.h>

#include "oslib.h"
#include "shooter_encoder.h"

/*-define--------------------------------------*/
#define AS5047P_READ_ANGLECOM       (0x3FFF|0x4000|0x8000)

/*-static variables-------------------------------*/
static const uint16_t shooter_encoder_tx_cmd = AS5047P_READ_ANGLECOM;
static const osThreadAttr_t shooter_encoder_task_attrs = {
    .name = "ShooterEncoder",
    .priority = osPriorityAboveNormal2,
    .stack_size = 128 * 4,
};

/*-static functions-------------------------------*/
static int spi_check_parity(uint16_t x) 
{
    x ^= x >> 8;
    x ^= x >> 4;
    x ^= x >> 2;
    x ^= x >> 1;
    return (~x) & 1;
}

static void shooter_encoder_task(void *argument)
{
    shooter_encoder_t *enc = (shooter_encoder_t *)argument;
    for (;;)
    {
        osDelay(10);
        HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive(enc->hspi, (uint8_t*)&shooter_encoder_tx_cmd, (uint8_t*)&enc->rx_data, 1, HAL_MAX_DELAY);
        HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET);
        if (spi_check_parity(enc->rx_data)) {
            int now_pos = enc->rx_data & 0x3FFF;
            now_pos = (int)(roundl((double)(now_pos) * 360 / 16384));
            int diff_pos = now_pos - enc->last_pos;
            enc->last_pos = now_pos;
            if (diff_pos > 180) {
                diff_pos -= 360;
            }
            else if (diff_pos < -180) {
                diff_pos += 360;
            }
            if (enc->target != NULL && enc->tar_mutex != NULL) {
                if (osOK == osMutexAcquire(enc->tar_mutex, 0)) {
                    (*enc->target) += diff_pos;
                    osMutexRelease(enc->tar_mutex);
                }
            }
        }
    }
}

/*-global functions-------------------------------*/
void shooter_encoder_init(shooter_encoder_t *enc, SPI_HandleTypeDef *hspi, int *target, osMutexId_t tar_mutex)
{
    enc->hspi = hspi;
    enc->last_pos = 0;
    enc->target = target;
    enc->tar_mutex = tar_mutex;
    // shooter_encoder_get_inc(enc);
    enc->task = osThreadNew(shooter_encoder_task, (void *)enc, &shooter_encoder_task_attrs);
}

int shooter_encoder_get_inc(shooter_encoder_t *enc)
{
    HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_RESET);
    HAL_SPI_TransmitReceive(enc->hspi, (uint8_t*)&shooter_encoder_tx_cmd, (uint8_t*)&enc->rx_data, 1, HAL_MAX_DELAY);
    HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET);
    if (spi_check_parity(enc->rx_data)) {
        int now_pos = enc->rx_data & 0x3FFF;
        // static int temp_count = 0;
        // if (temp_count++ % 10 == 0)
        //     // uprintf("%d %d\n", (int)(roundf((float)(now_pos)*360/AS5047P_MAX_NUM)), (int)(roundf((float)diff_pos*360/AS5047P_MAX_NUM)));
        //     uprintf("%x\n", now_pos);
        now_pos = (int)(roundl((double)(now_pos) * 360 / 16384));
        int diff_pos = now_pos - enc->last_pos;
        enc->last_pos = now_pos;
        
        if (diff_pos > 180) {
            diff_pos -= 360;
        }
        else if (diff_pos < -180) {
            diff_pos += 360;
        }
        return diff_pos;
    }
    else {
        return 0;
    }
}
